{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "muslim-trinidad",
   "metadata": {},
   "source": [
    "# New FMU interface in CasADi\n",
    "Joel Andersson (joel@jaeandersson.com)\n",
    "\n",
    "Last updated October 25, 2024"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "experienced-prayer",
   "metadata": {},
   "source": [
    "Contents:\n",
    " * Loading FMUs\n",
    " * Generating differentiable function objects from FMUs\n",
    "   - First order derivatives\n",
    "   - Second order derivatives\n",
    " * Simulating an FMU\n",
    " * Dynamic discretization - Implicit Runge-Kutta method\n",
    " * Solving nonlinear systems of equations\n",
    " * Forward/adjoint sentivity analysis\n",
    " * Solving nonlinear programs (NLPs) with IPOPT\n",
    " * Implementing dynamic optimization - direct collocation"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "stopped-alfred",
   "metadata": {},
   "source": [
    "## Introduction\n",
    "\n",
    "CasADi has two existing foreign function interfaces (FFI):\n",
    " * Low level CasADi's C API -- same as the C code generated from CasADi\n",
    " * High level interface based on polymorphism:\n",
    "   - Inherit from abstract base class, implement methods\n",
    "   - Works from C++, Python, MATLAB/Octave\n",
    " * Neither suitable for FMI\n",
    "   - CasADi functions are \"pure\" / state-less, FMI has state\n",
    "   - Existing interfaces assume dimensions and sparsity patterns known a priori, not the case for FMI\n",
    "\n",
    "The CasADi FMI interface represents a new way to import *differentiable* models into CasADi:\n",
    " * No external dependencies - interact directly with the FMU C API and XML API\n",
    " * Based on CasADi's existing interface to dynamically linked libraries (DLL)\n",
    "   - Mature feature in CasADi - works in Windows, OSX, Linux\n",
    "   - Compatible with CasADi's Clang-based just-in-time compiler\n",
    " * CasADi core feature: Use from C++, Python, MATLAB/Octave"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "automatic-contamination",
   "metadata": {},
   "source": [
    "## Modelica Example\n",
    "\n",
    "Consider the following simple dynamic model corresponding to a controlled Van der Pol oscillator:\n",
    "$$\n",
    "\\begin{array}{l}\n",
    "\\dot{x}_1 = (1-x_2^2) \\, x_1 - x_2 + u \\\\\n",
    "\\dot{x}_2 = x_1 \\\\\n",
    "\\end{array}\n",
    "$$\n",
    "with the initial conditions for the two states given by $x_1(0) = 0$, $x_2(0) = 1$. The model has a free control $u(t) \\in [-1, 1]$.\n",
    "\n",
    "We can write this model in Modelica as follows:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "outer-convenience",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Toy example: Van der Pol oscillator\n",
    "mofile = 'vdp.mo'\n",
    "with open(mofile, 'r') as myfile: print(myfile.read())"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "covered-argentina",
   "metadata": {},
   "source": [
    "## Compiling the model into an FMU\n",
    "Different tools exist to compile Modelica models like these into FMUs.\n",
    "\n",
    "Below is a code snippet for generating FMUs using two different Modelica compilers,\n",
    "the (closed-source) OCT compiler from Modelon and the open-source OpenModelica compiler:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "statistical-pepper",
   "metadata": {},
   "outputs": [],
   "source": [
    "compiler = 'Precompiled'\n",
    "#compiler = 'OCT'\n",
    "#compiler = 'OpenModelica'\n",
    "from time import time\n",
    "import os\n",
    "if compiler == 'Precompiled':\n",
    "  # The FMU has already been compiled, using e.g. Dymola, and is available in the directory\n",
    "  fmu_file = os.path.join(os.getcwd(), 'vdp.fmu')\n",
    "  print('Using precompiled FMU %s' % fmu_file)\n",
    "else:\n",
    "  # Start timer\n",
    "  t1 = time()\n",
    "  # Compile the FMU\n",
    "  if compiler == 'OCT':\n",
    "    # Compile using OCT, cf. OPTIMICA Compiler Toolkit (OCT) User's Guide\n",
    "    from pymodelica import compile_fmu\n",
    "    compiler_options = dict(generate_ode_jacobian = True)\n",
    "    fmu_file = compile_fmu('vdp', [mofile], compiler_options = compiler_options)\n",
    "  elif compiler == 'OpenModelica':\n",
    "    # Compile using OpenModelica, cf. the OpenModelica User's Guide\n",
    "    from OMPython import OMCSessionZMQ\n",
    "    omc = OMCSessionZMQ()\n",
    "    if omc.loadFile(mofile).startswith('false'):\n",
    "      raise Exception('Modelica compilation failed: {}'.format(omc.sendExpression('getErrorString()')))\n",
    "    omc.sendExpression('setDebugFlags(\"-disableDirectionalDerivatives\")')\n",
    "    fmu_file = omc.sendExpression('buildModelFMU ({})'.format('vdp'))\n",
    "    flag = omc.sendExpression('getErrorString()')\n",
    "    if not fmu_file.endswith('.fmu'): raise Exception('FMU generation failed: {}'.format(flag))\n",
    "    print(\"buildModelFMU  warnings:\\n{}\".format(flag))\n",
    "  else:\n",
    "    raise Exception('Unknown compiler: ' + compiler)\n",
    "  # Output results of compilation\n",
    "  print('Compiled %s into %s using %s in %g s' % (mofile, fmu_file, compiler, time() - t1))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "favorite-error",
   "metadata": {},
   "source": [
    "## Unzippling the FMU\n",
    "An FMU is essentially a ZIP archive containing a number of files, most importantly an XML file with\n",
    "meta information and a compiled dynamically linked libraries (DLLs) for one or more platforms.\n",
    "The format is described in the FMI specification available from the [fmi-standard.org](http://fmi-standard.org) website.\n",
    "\n",
    "In the following, we will assume an FMU of version 2.0 or version 3.0, which is what is supported for FMU import in CasADi as of this writing.\n",
    "\n",
    "CasADi 3.7 ships with a ZIP utility, so you can pass the name of the FMU directly. In earlier versions, before the FMU could be loaded, the user needed to unzip it. In Python, we can easily do the unzipping with the `zipfile` package:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "immune-question",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Path for the unzipped FMU\n",
    "import os\n",
    "unzipped_name = mofile[:mofile.find('.')]\n",
    "unzipped_path = os.path.join(os.getcwd(), unzipped_name)\n",
    "# Remove any existing directory with this name\n",
    "# This prevents memory corruption if the old DLL is still in memory\n",
    "import shutil\n",
    "if os.path.isdir(unzipped_path): shutil.rmtree(unzipped_path)  \n",
    "# Unzip\n",
    "import zipfile\n",
    "with zipfile.ZipFile(fmu_file, 'r') as zip_ref: zip_ref.extractall(unzipped_name)\n",
    "print('Unzipped %s into %s' % (fmu_file, unzipped_path))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "ethical-boards",
   "metadata": {},
   "source": [
    "## DaeBuilder instances\n",
    "The FMI interface in CasADi consists of two main components:\n",
    " - The `casadi::DaeBuilder` class which contains *mutable* interface to the model representation.\n",
    " - The `casadi::FmuFunction` class which contains an *immutable* interface to the model equations\n",
    "\n",
    "We can create a `casadi::DaeBuilder` instance from the XML component of the FMU as follows."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "objective-avenue",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Use FMU to create a CasADi/DaeBuilder instance\n",
    "import casadi\n",
    "t1 = time()\n",
    "dae = casadi.DaeBuilder('vdp', unzipped_path)\n",
    "print('Loaded %s in %g seconds' % (unzipped_path, time() - t1))\n",
    "dae.disp(True)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "93f80dfe",
   "metadata": {},
   "source": [
    "## Check analytic derivative support in FMU\n",
    "To efficiently use an FMU in dynamic simulation and optimization, we should use the analytic derivatives from the FMU.\n",
    "Analytic derivatives are an optional part of the FMU standard, so make sure that your FMUs support them:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d05b83d2",
   "metadata": {},
   "outputs": [],
   "source": [
    "print('FMU ({}) provides directional derivatives: {}'.format(fmu_file, dae.provides_directional_derivative()))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "205b9599",
   "metadata": {},
   "source": [
    "As of this writing, we have successfully imported FMUs with analytic derivatives generated in Dymola, OCT, and OpenModelica.\n",
    "We can proceed with the rest of the demo without analytic derivatives, but we do not recommend to rely on finite differences like this in actual applications."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "raised-honey",
   "metadata": {},
   "source": [
    "### Accessing meta information\n",
    "\n",
    "Once loaded, we can access (or change) the meta information available in the FMU XML:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "compact-breathing",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Get state vector, initial conditions, bounds\n",
    "x = dae.x()\n",
    "lbx = dae.min(x)\n",
    "ubx = dae.max(x)\n",
    "x0 = dae.start(x)\n",
    "print('x: ', x)\n",
    "print('lbx: ', lbx)\n",
    "print('ubx: ', ubx)\n",
    "print('x0: ', x0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "consistent-cambridge",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Get free control, bounds\n",
    "u = dae.u()\n",
    "lbu = dae.min(u)\n",
    "ubu = dae.max(u)\n",
    "print('u: ', u)\n",
    "print('lbu: ', lbu)\n",
    "print('ubu: ', ubu)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "alien-conclusion",
   "metadata": {},
   "source": [
    "## FMU function objects\n",
    "\n",
    "The evaluation of the FMU model equations takes place in a separate class, ``casadi::FmuFunction``, which is a standard differentiable CasADi function object. As such, it can embedded into optimization problems and have derivatives calculated automatically \"under-the-hood\".\n",
    "\n",
    "FmuFunction instances are created from DaeBuilder instances, but does not depend on them after creation. Instead, they make a copy of the relevant subset of meta information in the DaeBuilder instance and stores them in the instance. If the DaeBuilder instance is later changed or deleted, it will not affect the FmuFunction. Included in the `FmuFunction` is a binary interface to the DLLs, adhering to the FMU standard (assuming version 2.0 as of this writing).\n",
    "\n",
    "In the simplest case, let us create a function object that calculate the ODE right-hand-side as a function of the state vector $x$ and the controls $u$:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "creative-graduation",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Evaluate ODE right-hand-side\n",
    "f = dae.create('f', ['x', 'u'], ['ode'])\n",
    "print(f)\n",
    "# Evaluate the function numerically\n",
    "u_test = 0.4\n",
    "xdot_test = f(x0, u_test)\n",
    "print('xdot_test: ', xdot_test)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "descending-wrong",
   "metadata": {},
   "source": [
    "The creation of function objects is general-purpose and it is up to the user to decide what inputs or output vectors are relevant. For example, a steady-state optimization might not have an $x$ vector at all.\n",
    "In other cases, we may want to change the standard meaning of variables, for example:\n",
    " * We may want to declare free parameters as (time-varying) control inputs in our Modelica sources, since Modelica tools may not calculate derivatives with respect to parameters.\n",
    " * We can break up algebraic loops inside a Modelica code by producing a new input/output pair. The input variable will then be treated as an algebraic variable in CasADi and the output will define a residual equation. In the optimization/simulation formulation, we can then use a DAE formulation instead of an ODE formulation. This approach is sometimes referred to as *hand-guided tearing* (HGT).\n",
    "\n",
    "Only real-valued variables that can be expected to change over subsequent function evaluations should be included in the list of inputs and outputs. For parameters that do not change during subsequent calls to the FMU -- e.g. from an optimization or simulation algorithm -- the user should set these variables in the DaeBuilder instance *before* the function creation:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "controversial-lemon",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Alternative ODE right-hand-side function with u fixed\n",
    "dae.set('u', 0.4)\n",
    "f_no_u = dae.create('f_no_u', ['x'], ['ode'])\n",
    "print(f_no_u)\n",
    "# Evaluate the function numerically\n",
    "xdot_test_no_u = f_no_u(x0)\n",
    "print('xdot_test_no_u: ', xdot_test_no_u)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "acute-thanksgiving",
   "metadata": {},
   "source": [
    "Similarly, only real-valued, differentiable variables should be included in the list of function outputs. Other variables can be included as \"auxilliary variables\" by specifying the \"aux\" option - the requested values will then be retrieved from the *stats*:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "prerequisite-rouge",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Evaluate ODE right-hand-side with auxilliary field\n",
    "f_with_aux = dae.create('f_with_aux', ['x', 'u'], ['ode'],\n",
    "               dict(aux = ['x1', 'x2', 'u']))\n",
    "print(f_with_aux)\n",
    "# After evaluation, the aux variables are in the stats field:\n",
    "u_test = 0.4\n",
    "xdot_test_with_aux = f_with_aux(x0, u_test)\n",
    "print('xdot_test_with_aux: ', xdot_test_with_aux)\n",
    "print(f_with_aux.stats())"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "39d600b2",
   "metadata": {},
   "source": [
    "Anything that is not real-valued (but for example string or integer valued) can also be passed this way."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "danish-ownership",
   "metadata": {},
   "source": [
    "### First order derivatives - Jacobians\n",
    "\n",
    "First order derivative information can be obtained either numerically or analytically. In either case, the Jacobian sparsity information in the FMU will be exploited using CasADi's graph coloring techniques.\n",
    "\n",
    "For efficiency, the evaluation can be done in parallel using either OpenMP or std::thread. The interface will distribute the evaluation over the available threads:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "major-plane",
   "metadata": {},
   "outputs": [],
   "source": [
    "# A function that calculates the ODE and two Jacobian blocks:\n",
    "J = dae.create('J', ['x', 'u'], ['ode', 'jac_ode_x', 'jac_ode_u'],\n",
    "              dict(verbose = True, parallelization = 'openmp'))\n",
    "print(J)\n",
    "\n",
    "# Evaluate Jacobian\n",
    "xdot_test, A_test, B_test = J(x0, u_test)\n",
    "print('xdot_test: ', xdot_test)\n",
    "print('A_test: ', A_test)\n",
    "print('B_test: ', B_test)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "centered-picture",
   "metadata": {},
   "source": [
    "Notice that the output is sparse (00 denotes a stuctural zero in CasADi).\n",
    "\n",
    "By default, analytic derivatives will be used if they are available in the FMU. Alternatively, derivatives can be calculated using finite differences using one of three schemes:\n",
    " * Forward differences\n",
    " * Central differences\n",
    " * A smoothness seeking scheme using 5-point stencils: $f(x-2h), f(x-h), f(x), f(x+h), f(x+2h)$\n",
    " \n",
    "All schemes will use perturbation sizes scaled by the nominal values in the FMU. If FMU variable bounds are present, they will be respected.\n",
    "This makes the perturbation step size predicable and the user can influence individual step sizes by modifying the corresponding nominal value."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "wanted-projection",
   "metadata": {},
   "outputs": [],
   "source": [
    "# A function that calculates the ODE and two Jacobian blocks:\n",
    "J_fd = dae.create('J_fd', ['x', 'u'], ['ode', 'jac_ode_x', 'jac_ode_u'],\n",
    "              dict(verbose = True,\n",
    "                   parallelization = 'openmp',\n",
    "                   enable_ad = False))\n",
    "print(J_fd)\n",
    "\n",
    "# Evaluate Jacobian\n",
    "xdot_fd, A_fd, B_fd = J_fd(x0, u_test)\n",
    "print('xdot_fd: ', xdot_fd)\n",
    "print('A_fd: ', A_fd)\n",
    "print('B_fd: ', B_fd)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "vietnamese-venue",
   "metadata": {},
   "source": [
    "Finally, the finite difference implementation can be run in \"shadow mode\" by setting the option \"validate_ad\" to True. The AD derivatives are then compared with the finite difference values and warnings are emitted if the difference exceeds a threshold. This feature is intended to help ensuring that the analytic derivatives are correct for complex physical models."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "subject-myanmar",
   "metadata": {},
   "source": [
    "### Second order derivatives - Hessians\n",
    "The interface can also calculate second derivatives, i.e. Hessians. The Hessian can be calculated for any linear combination of the outputs (in gradient-based optimization, the weights are typically Lagrange multipliers).\n",
    "\n",
    "Using the \"dependenciesKind\" field in the FMI XML, we can determine which input variables enter nonlinearly in any of the used outputs variables. This allows a \"worst-case\" sparsity pattern of the Hessian to be constructed.\n",
    "\n",
    "The Hessian is then calculated by applying finite differencing to the first order analytic derivatives. The first order analytic derivatives are, in turn, by implicitly multiplying the Jacobian with a vector from the left, where the Jacobian is calculated using the graph coloring techniques described above. This is *not* the most efficient way to calculate second order derivatives -- a much more efficient alternative is to use adjoint derivatives to calculate the first order derivatives before using either finite differences or AD for the second derivatives. Since adjoint derivatives are not available in FMI 2.0, the workaround using the calculation of the sparse Jacobian was used. If/when the FMI interface is updated to support FMI 3.0, the first order derivatives can be calculated efficiently using `fmi3GetAdjointDerivative`, if supported by the generating tool. The current gradient calculation approach can then instead be used as a diagnostics feature, checking that `fmi3GetAdjointDerivative` for correctness for a specific model.\n",
    "\n",
    "The Hessian calculation can be done in parallel, using OpenMP or std::thread. The nonlinearly entering variables are then split over the available threads:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "grateful-agent",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create a function that calculate the Hessian of a linear\n",
    "# combination of \"ode\" with respect to x and u: \n",
    "H = dae.create('H', ['x', 'u', 'adj_ode'],\n",
    "               ['jac_adj_x_x', 'jac_adj_x_u', 'jac_adj_u_u'],\n",
    "               dict(verbose = True, parallelization = 'openmp'))\n",
    "print(H)\n",
    "# Evaluate Hessian\n",
    "# Evaluate Jacobian\n",
    "H_xx, H_xu, H_uu = H(x0, u_test, 1)\n",
    "print('H_xx: ', H_xx)\n",
    "print('H_xu: ', H_xu)\n",
    "print('H_uu: ', H_uu)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "sublime-boxing",
   "metadata": {},
   "source": [
    "In general, we have to perturb all nonlinearly entering variables, which will give us the full Hessian, not just the upper or lower triangular part. But since we also know that the Hessian has to be symmetric, we can use a symmetry check as an consistency check for the Hessian calculation. In addition, the diagonal entries of the Hessian can be calculated by using second order finite differences. Both these calculations can be done without any additional calls to the FMU, i.e. without significant computational overhead, and can thus be an important check that the entries are correctly calculated.\n",
    "\n",
    "This diagnostics feature can be enabled by setting the option \"check_hessian\" to True. It is especially important since the correctness of the calculation will depend on suitably chosen nominal values, for scaling the perturbation step sizes. It is also important to be able to detect non-smoothnesses in the model equations or issues with the tool used to generate the FMU."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "foster-transcription",
   "metadata": {},
   "source": [
    "## Improved ODE/DAE integrator class suitable for FMI simulation\n",
    " * CasADi's ODE/DAE integrators previously only efficient for forward/adjoint sensitivity calculations\n",
    "    when the ODEs/DAEs were available as *expressions*, not general *functions*\n",
    " * With CasADi 3.6, the `integrator` class also works efficiently for DAEs given as functions, including `FmuFunction` instances\n",
    " * In addition, ODE/DAE sensitivity analysis has been greatly refactored in CasADi 3.6:\n",
    "    - It now supports piecewise constant controls (\"u\")\n",
    "    - The differentiation now works when the solution is retrieved at a time grid, not just a single point\n",
    "    - The integrator plugin classes much better exploit the ODE/DAE sensitivity analysis structure"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "buried-pennsylvania",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "# Create an integrator for simulating the ODE over 10 s using CVODES\n",
    "T = 10.\n",
    "tgrid = np.linspace(0., T, 100)\n",
    "dae.set_min(x[0], -np.inf) # relax lower bound on x1\n",
    "daefun = dae.create('daefun')\n",
    "sim = casadi.integrator('sim', 'cvodes', daefun, 0, tgrid)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bridal-helicopter",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Call integrator instance\n",
    "r = sim(x0 = x0, u = u_test)\n",
    "x_traj_sim = r['xf'].full()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "moving-wealth",
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "# Plot x1 vs. x2\n",
    "plt.plot(x_traj_sim[0,:], x_traj_sim[1,:])\n",
    "plt.xlabel(x[0])\n",
    "plt.ylabel(x[1])\n",
    "plt.grid(True)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "439b1e1c",
   "metadata": {},
   "source": [
    "## Forward sensitivity analysis with Sundials/CVODES\n",
    "In the following, we show how forward sensitivities can be calculated analytically for the FMU simulation.\n",
    "We will formulate these equations by providing the \"nfwd\" argument to the integrator instance.\n",
    "The `nfwd` argument will give additional columns in the input and output matrices corresponding to sensitivities\n",
    "that will be calculated along with the non-differentiated solution.\n",
    "\n",
    "This is a new, lower level interface, to ODE/DAE sensitivity available in CasADi 3.6.\n",
    "You can also use the standard sensitivity syntax as demonstrated in the sensitivity_analysis.py example.\n",
    "We use the low-level syntax here since it is easier for debugging."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "908e76ab",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create a new Simulator with support for forward sensitivities\n",
    "fwd_sim = casadi.integrator('fwd_sim', 'cvodes', daefun, 0, tgrid, dict(nfwd = 1, verbose = False, print_stats = True, second_order_correction = False))\n",
    "print(fwd_sim)\n",
    "# Let's calculate sensitivity w.r.t. x1, i.e. use a forward seed [1, 0] for x0:\n",
    "seed_x0 = casadi.DM([1, 0])\n",
    "fwd_test = fwd_sim(x0 = casadi.horzcat(x0, seed_x0),\n",
    "                  u = casadi.horzcat(u_test, 0))\n",
    "# The directional derivatives can be found in column 1, 3, 5, etc. of `xf` for the different grid points.\n",
    "# (column 0, 2, 4, etc corresponds to nondifferentiated xf, for each grid point)\n",
    "print('dxf/dx0 * seed_x0 [AD, forward] = ', fwd_test['xf'][:, 1::2])\n",
    "# We can compare this result with a finite difference perturbation\n",
    "pert = 1e-3\n",
    "pert_sim = sim(x0=x0 + pert * seed_x0, u=u_test)\n",
    "x_pert_sim = pert_sim['xf']\n",
    "print('dxf/dx0 * seed_x0 [FD] = ', (x_pert_sim - x_traj_sim) / pert)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "7aafc625",
   "metadata": {},
   "source": [
    "## Adjoint sensitivity analysis with Sundials/CVODES\n",
    "In the following, we show how adjoint sensitivities can be calculated analytically for the FMU simulation.\n",
    "We will formulate these equations by providing the \"nadj\" argument to the integrator instance.\n",
    "The `nadj` argument will give additional columns in the inputs and outputs of the integrator. Each seed has now\n",
    "the name of the nondifferentiated output, prefixed with \"adj_\" and each sensitivity has the name of the\n",
    "nondifferentiated input, also prefixed with \"adj_\".\n",
    "\n",
    "As in the forward sensitivity case, you can also use the standard sensitivity syntax as demonstrated in the\n",
    "sensitivity_analysis.py example."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "63da7283",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create a new Simulator with support for adjoint sensitivities\n",
    "adj_sim = casadi.integrator('adj_sim', 'cvodes', daefun, 0, tgrid, dict(nadj = 1, verbose = False, print_stats = True, second_order_correction = True, print_time = True))\n",
    "print(adj_sim)\n",
    "# Let's calculate sensitivity of xf[0] w.r.t. u at all the grid points, i.e. use an adjoint seed [1, 0] for xf:\n",
    "seed_xf = casadi.DM.zeros(2, 100)\n",
    "seed_xf[:, -1] =  casadi.DM([1, 0])  # We seed the state at the last grid point only\n",
    "adj_test = adj_sim(x0 = x0, adj_xf = seed_xf, u = u_test)\n",
    "print('trans(dxf/du) * seed_xf (AD, adjoint) = ', adj_test['adj_u'])"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "e4a9798e",
   "metadata": {},
   "source": [
    "## Forward-over-adjoint sensitivity analysis with Sundials/CVODES\n",
    "We can also calculate second order sensitivities, i.e. Hessian-times-vector products, analytically. Again, we use the new low-level interface"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "08ccbc17",
   "metadata": {},
   "outputs": [],
   "source": [
    "fwd_adj_sim = casadi.integrator('fwd_adj_sim', 'cvodes', daefun, 0, tgrid, dict(nfwd = 1, nadj = 1, verbose = False, print_stats = True, second_order_correction = False))\n",
    "print(fwd_adj_sim)\n",
    "# Let's calculate sensitivity of d(xf[0])/dx1 w.r.t. u at all the grid points, i.e. combining the seeds for forward and adjoint sensitivitiy analysis above\n",
    "fwd_adj_test = fwd_adj_sim(x0 = casadi.horzcat(x0, seed_x0), adj_xf = casadi.horzcat(seed_xf, casadi.DM.zeros(seed_xf.shape)), u = casadi.horzcat(u_test, 0))\n",
    "print('d(trans(dxf/du) * seed_xf)/dx0 * seed_x0 (AD, forward-over-adjoint) = ', fwd_adj_test['adj_u'][:,1::2])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "greek-kingdom",
   "metadata": {},
   "source": [
    "### Dynamic discretization - Solving nonlinear system of equations"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "violent-sending",
   "metadata": {},
   "source": [
    "ODEs from Modelica are typically sparse. Let us define a implicit fixed step\n",
    "integrator suitable for stiff ODEs and DAEs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "silent-disclaimer",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Number of integrator steps\n",
    "N = 50\n",
    "# Size of the finite elements\n",
    "h = T/N"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "norman-convert",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Let us use a 4th order collocation discretization using Legendre roots, cf. \n",
    "# Nonlinear Programming: Concepts, Algorithms, and Applications to Chemical Processes\n",
    "# by Lorenz Biegler (2010)\n",
    "d = 3\n",
    "# The roots can be queried from CasADi or looked up in the above textbook\n",
    "tau_root = np.append(0, casadi.collocation_points(d, 'legendre'))\n",
    "# Coefficients of the collocation equation\n",
    "C = np.zeros((d+1,d+1))\n",
    "# Coefficients of the continuity equation\n",
    "D = np.zeros(d+1)\n",
    "# Coefficients of the quadrature function\n",
    "B = np.zeros(d+1)\n",
    "# Construct polynomial basis\n",
    "for j in range(d+1):\n",
    "    # Construct Lagrange polynomials\n",
    "    p = np.poly1d([1])\n",
    "    for r in range(d+1):\n",
    "        if r != j:\n",
    "            p *= np.poly1d([1, -tau_root[r]]) / (tau_root[j]-tau_root[r])\n",
    "    # Evaluate at the final time to get the coefficients of the continuity equation\n",
    "    D[j] = p(1.0)\n",
    "    # Evaluate the time derivative to get the coefficients of the continuity equation\n",
    "    pder = np.polyder(p)\n",
    "    for r in range(d+1): C[j,r] = pder(tau_root[r])\n",
    "    # Evaluate the integral of the polynomial to get the coefficients of the quadrature function\n",
    "    pint = np.polyint(p)\n",
    "    B[j] = pint(1.0)\n",
    "print('tau_root: ', tau_root)\n",
    "print('C: ', C)\n",
    "print('D: ', D)\n",
    "print('B: ', B)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "major-shelf",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Symbolic expression for the controls (piecewise constant)\n",
    "U = casadi.MX.sym('U',len(u))\n",
    "# Symbolic expressions for the states at each collocation point\n",
    "X = [casadi.MX.sym('X_' + str(j), len(x)) for j in range(d+1)]\n",
    "# Define the collocation equations\n",
    "g = []\n",
    "for j in range(1,d+1):\n",
    "  # Expression for the state derivative at the collocation point\n",
    "  xdot_j = 0\n",
    "  for r in range (d+1): xdot_j += C[r,j]*X[r]\n",
    "  # Append collocation equations\n",
    "  g.append(h*f(X[j],U) - xdot_j)\n",
    "# Concatenate constraints\n",
    "g = casadi.vertcat(*g)\n",
    "# Form a root-finding function, implicitly defining X[1:] as a function of U, X[0]\n",
    "X_unknown = casadi.vertcat(*X[1:])\n",
    "rfp = casadi.Function('rfp', [X_unknown, X[0], U], [g], ['V', 'X0', 'U'], ['res'])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "accessible-journal",
   "metadata": {},
   "source": [
    "* Rootfinding instances in CasADi solves problems of the form:\n",
    " $$ g(x, p) = 0 $$\n",
    "* Nonlinear system of equation can be solved with KINSOL, other Newton solvers\n",
    "* Rootfinding instances are differentiable, derivatives to any order can be derived from implicit-function theorem"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "social-illustration",
   "metadata": {},
   "outputs": [],
   "source": [
    "# We can solve this system of equations using Sundials/KINSOL\n",
    "ifcn = casadi.rootfinder('ifcn', 'kinsol', rfp, dict(print_level = 1))\n",
    "# Take a single step of the integrator\n",
    "v0 = casadi.repmat(x0, d, 1)\n",
    "v0 = ifcn(v0, x0, u_test)\n",
    "# State at each collocation point\n",
    "x_all = casadi.reshape(v0, len(x), d)\n",
    "# Prepend initial state\n",
    "x_all = casadi.horzcat(x0, x_all)\n",
    "# Also calculate the state at the end\n",
    "xf = casadi.mtimes(x_all, D)\n",
    "# Print solution\n",
    "print('x_all: ', x_all)\n",
    "print('xf: ', xf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "valid-lithuania",
   "metadata": {},
   "outputs": [],
   "source": [
    "# We can also use CasADi's native Newton solver\n",
    "ifcn = casadi.rootfinder('ifcn', 'newton', rfp, dict(print_iteration = True))\n",
    "# Take a single step of the integrator\n",
    "v0 = casadi.repmat(x0, d, 1)\n",
    "v0 = ifcn(v0, x0, u_test)\n",
    "# State at each collocation point\n",
    "x_all = casadi.reshape(v0, len(x), d)\n",
    "# Prepend initial state\n",
    "x_all = casadi.horzcat(x0, x_all)\n",
    "# Also calculate the state at the end\n",
    "xf = casadi.mtimes(x_all, D)\n",
    "# Print solution\n",
    "print('x_all: ', x_all)\n",
    "print('xf: ', xf)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "infinite-bullet",
   "metadata": {},
   "source": [
    "### Solvers for nonlinear systems of equations are differentiable to any order in CasADi:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "appropriate-weekly",
   "metadata": {},
   "outputs": [],
   "source": [
    "# We can solve this system of equations using Sundials/KINSOL\n",
    "ifcn = casadi.rootfinder('ifcn', 'kinsol', rfp, dict(print_level = 1))\n",
    "# Differentiate Newton solver\n",
    "jac_ifcn = ifcn.jacobian()\n",
    "sol = jac_ifcn(V0 = v0, X0 = x0, U = u_test)\n",
    "# Jacobian of state at collocation points w.r.t. U\n",
    "print('jac_V_U = ', sol['jac_V_U'])\n",
    "# Jacobian of state at collocation points w.r.t. X0\n",
    "print('jac_V_X0 = ', sol['jac_V_X0'])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "assisted-coupon",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compare with finite differences\n",
    "pert = 1e-3\n",
    "# Perturn u\n",
    "v0_pert = ifcn(casadi.repmat(x0, d, 1), x0, u_test + pert)\n",
    "jac_V_U_fd = (v0_pert - v0) / pert\n",
    "print('jac_V_U (FD) = ', jac_V_U_fd)\n",
    "# Perturb x1\n",
    "v0_pert = ifcn(casadi.repmat(x0, d, 1), x0 + casadi.DM([pert, 0]), u_test)\n",
    "jac_V_X0_fd1 = (v0_pert - v0) / pert\n",
    "# Perturb x2\n",
    "v0_pert = ifcn(casadi.repmat(x0, d, 1), x0 + casadi.DM([0, pert]), u_test)\n",
    "jac_V_X0_fd2 = (v0_pert - v0) / pert\n",
    "print('jac_V_X0 (FD) = ', casadi.horzcat(jac_V_X0_fd1, jac_V_X0_fd2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "sealed-contractor",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Let's create a CasADi function for simulating the whole trajectory using Kinsol\n",
    "ifcn = casadi.rootfinder('ifcn', 'kinsol', rfp)\n",
    "x0_in = casadi.MX.sym('x0', len(x))\n",
    "u_all = []\n",
    "xk = x0_in\n",
    "x_all = [x0_in]\n",
    "for k in range(N):\n",
    "    # Symbolic expression for the control for the interval\n",
    "    uk = casadi.MX.sym('u' + str(k), len(u))\n",
    "    u_all.append(uk)\n",
    "    # Solve rootfinding problem, using previous step as guess\n",
    "    vk = ifcn(casadi.repmat(xk,d,1), xk, uk)\n",
    "    # Reshape and prepend initial state\n",
    "    vk = casadi.reshape(vk, len(x), d)\n",
    "    vk = casadi.horzcat(xk, vk)\n",
    "    # Get the state at the end\n",
    "    xk = casadi.mtimes(vk, D)\n",
    "    # Save trajectory\n",
    "    x_all.append(xk)\n",
    "# Embed in a CasADi Function\n",
    "irksim = casadi.Function('irksim', [x0_in, casadi.vcat(u_all)], [casadi.hcat(x_all)],\n",
    "                         ['x0', 'u'], ['x'])\n",
    "# Evaluate the function\n",
    "x_traj = irksim(x0, u_test).full()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "confused-situation",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Plot x1 vs. x2\n",
    "plt.clf()\n",
    "plt.plot(x_traj_sim[0,:], x_traj_sim[1,:], 'o', label = 'CVodes')\n",
    "plt.plot(x_traj[0,:], x_traj[1,:], '-', label = 'Implicit Runge-Kutta with Kinsol')\n",
    "plt.xlabel(x[0])\n",
    "plt.ylabel(x[1])\n",
    "plt.grid(True)\n",
    "plt.legend()"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "freelance-swift",
   "metadata": {},
   "source": [
    "## ODE/DAE sensitivity analysis of custom fixed step integrator"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "beautiful-forum",
   "metadata": {},
   "outputs": [],
   "source": [
    "# A nonlinear equation solver is a differentiable object in CasADi.\n",
    "# We can differentiate it analytically to get the Jacobian-times-vector product:\n",
    "fwd_irksim = irksim.forward(1)\n",
    "# Let's calculate sensitivity w.r.t. x1, i.e. use a forward seed [1, 0] for x0:\n",
    "seed_x0 = casadi.DM([1, 0])\n",
    "fwd_test = fwd_irksim(x0 = x0, u = u_test, fwd_x0 = seed_x0)\n",
    "print('fwd_x (AD, forward) = ', fwd_test['fwd_x'])\n",
    "# We can compare this result with a finite difference perturbation\n",
    "pert = 1e-3\n",
    "x_pert = irksim(x0 + pert * seed_x0, u_test).full()\n",
    "print('fwd_x (FD) = ', (x_pert - x_traj) / pert)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "polyphonic-request",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Now let's to a reverse mode AD, i.e. a transposed-Jacobian-times-vector product\n",
    "adj_irksim = irksim.reverse(1)\n",
    "# We seed the last entry in x2\n",
    "seed_x = casadi.DM.zeros(2, N + 1)\n",
    "seed_x[1, N] = 1\n",
    "adj_test = adj_irksim(x0 = x0, u = u_test, adj_x = seed_x)\n",
    "print('adj_x0 (AD, reverse) = ', adj_test['adj_x0'])\n",
    "print('adj_u (AD, reverse) = ', adj_test['adj_u'])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "confidential-argument",
   "metadata": {},
   "outputs": [],
   "source": [
    "# We can check that the result is consistent with forward mode AD:\n",
    "print('Forward mode: derivative of x[1, N] w.r.t. x0[0] = ', fwd_test['fwd_x'][1, N])\n",
    "print('Reverse mode: derivative of x[1, N] w.r.t. x0[0] = ', adj_test['adj_x0'][0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "basic-finding",
   "metadata": {},
   "outputs": [],
   "source": [
    "# We can also calculate the full Jacobian of x w.r.t. x0 and u:\n",
    "jac_irksim = irksim.jacobian()\n",
    "jac_test = jac_irksim(x0 = x0, u = u_test)\n",
    "print('jac_x_x0 = ', jac_test['jac_x_x0'])\n",
    "print('jac_x_u = ', jac_test['jac_x_u'])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "accepting-atlanta",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Note the sparsity pattern of dx/du:\n",
    "plt.spy(jac_test['jac_x_u'].sparse())\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "changing-james",
   "metadata": {},
   "source": [
    "## Dynamic optimization\n",
    "Now let us find the control signals that minimize the cost function\n",
    "$$ J := \\sum_{k = 0}^{N}{\\|u_k\\|^2 + \\|x_{k+1}\\|^2} $$\n",
    "We will embed the implicit Runge-Kutta discretization in a nonlinear program (NLP), which results in a simultaneous method for dynamic optimization. For details, cf. e.g. Biegler (2010)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ancient-ozone",
   "metadata": {},
   "outputs": [],
   "source": [
    "# We will construct an NLP incrementally, starting with no decision variables, \n",
    "# and constraints, and zero objective\n",
    "w = []\n",
    "g = []\n",
    "J = 0\n",
    "# Loop over all control intervals, constructing the NLP and x, u trajectories\n",
    "U_all = []\n",
    "X_all = []\n",
    "xk = casadi.MX(x0)\n",
    "for k in range(N):\n",
    "    # Symbolic expression for the control for the interval\n",
    "    uk = casadi.MX.sym('u' + str(k), len(u))\n",
    "    U_all.append(uk)\n",
    "    w.append(uk)\n",
    "    # States at each collocation point for the interval\n",
    "    Xc = [xk]\n",
    "    for j in range(1, d + 1):\n",
    "        xkj = casadi.MX.sym('x' + str(k) + '_' + str(j), len(x))\n",
    "        Xc.append(xkj)\n",
    "        X_all.append(xkj)\n",
    "        w.append(xkj)\n",
    "    # Collect the collocation equations\n",
    "    for j in range(1,d+1):\n",
    "      # Expression for the state derivative at the collocation point\n",
    "      xdot_j = 0\n",
    "      for r in range (d+1): xdot_j += C[r,j]*Xc[r]\n",
    "      # Append collocation equations\n",
    "      g.append(h*f(Xc[j],uk) - xdot_j)\n",
    "    # State at the end of the interval\n",
    "    xk_end = 0\n",
    "    for j in range(0,d+1): xk_end = xk_end + D[j] * Xc[j]\n",
    "    # New variable for the state at the end of the interval\n",
    "    xk = casadi.MX.sym('x' + str(k + 1), len(x))\n",
    "    X_all.append(xk)\n",
    "    w.append(xk)\n",
    "    # Enforce continuity\n",
    "    g.append(xk_end - xk)\n",
    "    # Add contributions to the objective\n",
    "    J = J + casadi.sumsqr(uk) + casadi.sumsqr(xk)\n",
    "# Concatenate vectors\n",
    "w = casadi.vcat(w)\n",
    "g = casadi.vcat(g)\n",
    "X_all = casadi.hcat(X_all)\n",
    "U_all = casadi.hcat(U_all)\n",
    "# Create mappings from (u, x) -> (w) and back\n",
    "to_w = casadi.Function('to_w', [X_all, U_all], [w], ['x', 'u'], ['w'])\n",
    "from_w = casadi.Function('from_w', [w], [X_all, U_all], ['w'], ['x', 'u'])\n",
    "# Form the NLP\n",
    "nlp = dict(x = w, f = J, g = g)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "adjacent-masters",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Let's form the Jacobian of g w.r.t. w\n",
    "t0 = time()\n",
    "jac_g_w = casadi.jacobian(g, w)\n",
    "print('Symbolic expression for Jacobian of g w.r.t w formed in %g s' % (time() - t0))\n",
    "# Create a function for evaluation\n",
    "t0 = time()\n",
    "jfcn = casadi.Function('jfcn', [w], [jac_g_w], ['w'], ['jac_g_w'])\n",
    "print('Jacobian function formed in %g s' % (time() - t0))\n",
    "# Evaluate function\n",
    "w_test = to_w(u = u_test, x = x0)['w']\n",
    "t0 = time()\n",
    "J_test = jfcn(w_test)\n",
    "print('Jacobian evaluated in %g s' % (time() - t0))\n",
    "# Let's look at the sparsity pattern\n",
    "plt.spy(J_test.sparse())\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "perfect-samoa",
   "metadata": {},
   "source": [
    "Formulate as a noninear program. In CasADi, they take the form:\n",
    " $$ \\begin{array}{cl}\n",
    " \\text{minimize} &  f(x, p) \\\\\n",
    "    \\text{subject to} & \\underline{x} \\le x \\le \\overline{x} \\\\\n",
    "     & \\underline{g} \\le g(x, p) \\le \\overline{g} \\\\\n",
    "     & p = \\underline{\\overline{p}}\n",
    "    \\end{array}\n",
    "     $$\n",
    "     \n",
    "CasADi will generate efficient derivative information and pass it to the specific solver\n",
    "\n",
    "Multiple solver plugins, including IPOPT:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "infrared-trial",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create an IPOPT instance, using L-BFGS\n",
    "opts = dict(ipopt = dict(hessian_approximation = 'exact', linear_solver = 'mumps'))\n",
    "solver = casadi.nlpsol('solver', 'ipopt', nlp, opts)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "scientific-console",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Generate initial guess for w\n",
    "w0 = to_w(u = 0, x = x0)['w']\n",
    "# Lower bound for w\n",
    "lbw = to_w(u = lbu, x = lbx)['w']\n",
    "# Upper bound for w\n",
    "ubw = to_w(u = ubu, x = ubx)['w']\n",
    "# Solve NLP\n",
    "sol = solver(x0 = w0, lbx = lbw, ubx = ubw, lbg = 0, ubg = 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "aab6c46d",
   "metadata": {},
   "outputs": [],
   "source": [
    "sol"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "logical-tobacco",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Extract solution\n",
    "w_sorted = from_w(w = sol['x'])\n",
    "x_opt = w_sorted['x']\n",
    "u_opt = w_sorted['u']\n",
    "# Add x0 to the beginning of x\n",
    "x_opt = casadi.horzcat(x0, x_opt)\n",
    "# Only keep the points at the interval boundaries\n",
    "x_opt = x_opt[:, 0::d+1]\n",
    "# Convert to numpy arrays\n",
    "x_opt = x_opt.full()\n",
    "u_opt = u_opt.full()\n",
    "# Plot solution\n",
    "plt.figure()\n",
    "plt.plot(x_opt[0,:], x_opt[1,:])\n",
    "plt.xlabel(x[0])\n",
    "plt.ylabel(x[1])\n",
    "plt.title(\"Optimal state trajectory\")\n",
    "plt.grid(True)\n",
    "# Also plot as a function of time\n",
    "grid = np.linspace(0, T, N+1)\n",
    "# Plot the results\n",
    "plt.figure()\n",
    "plt.clf()\n",
    "plt.plot(grid, x_opt[0,:],'--', label = x[0])\n",
    "plt.plot(grid, x_opt[1,:],'-.', label = x[1])\n",
    "plt.step(grid, np.concatenate(([np.nan], u_opt[0,:])),'-', label = u[0])\n",
    "plt.title(\"Optimal solution\")\n",
    "plt.xlabel('time')\n",
    "plt.legend()\n",
    "plt.grid()"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "rubber-stick",
   "metadata": {},
   "source": [
    "## Summary\n",
    " * New general-purpose FMI interface to CasADi\n",
    "   - Mutable interface to XML: DaeBuilder\n",
    "   - Function interface to DLL: FmuFunction\n",
    " * Jacobian calculation:\n",
    "   - Sparsity exploitation\n",
    "   - Parallelized\n",
    "   - AD or FD\n",
    "   - Higher order FD schemes\n",
    "   - FD respects variable bounds\n",
    "   - User can control step sizes by modifying nominal values\n",
    "   - Diagnostics, e.g. using FD to validate AD\n",
    " * Hessian calculation:\n",
    "   - AD for first order, FD for second order\n",
    "   - Sparsity information using \"DependenciesKind\" in FMU\n",
    "   - Parallelized\n",
    "   - Diagnostics, e.g. symmetry check\n",
    " * Generalized integrator class in CasADi\n",
    " * Proof-of-concept adjoint sensitivity analysis\n",
    " * Proof-of-concept dynamic optimization with second derivatives"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
